/**
 @file motores.c
 @brief Decsarrollo de las las funciones para el manejo de los motores EMG30 con reductora 36:1 utilizando una controladora MD25 y ruedas S360182.
 @date Agosors, 2011.
 @author José Delgado Pérez josedelgado@ula.ve josed43@gmail.com.

 Algunos datos: Velocidad maxima del motor 7200 rpm, con la reductora de 36:1 queda en 200 rpm en el eje,  a segundos es 3.333 rps.
 El diametro de la ruedas es 0.1 mts, lo que implica que la velocidad maxima es (0.05mts*PI*2)*(0.333 rev/seg)= 1.047197 mts/seg.
*/

#include <math.h>
#include <stdio.h>
#include "md25.h"
#include "motores.h"


#define DEBUG

/******Funiones de manipulacion de los motores EMG30 ******/

int iniciarComunicacionMotores(int *fd){
	int respuesta;
	respuesta = abrirPuerto(fd);
	if(respuesta == 0){
		//Modo de trabajo 3 Modalidad de giro con rancos de valores etre "-128" -- "0" -- "127"
		respuesta = respuesta + asignarModoVelocidad(*fd, MODO_VELOCIDAD);
		//(modo, tiempo_de_aceleracion) = (1,6.375 seg); (2,1.6 seg); (3,0.675 seg); (5,1.275 seg); (10,0.65 seg).		
		respuesta = respuesta + asignarModoAceleracion(*fd, MODO_ACELERACION);
		respuesta = respuesta + enviarComando(*fd, REINICIO_CODIFICADORES);
		respuesta = enviarComando(*fd,0x32);
		if(respuesta == 0){	
			return (0);
		}else{
			#ifdef DEBUG
			perror("iniciarComunicacionMotores: No se logro insertar la configuracion de trabajo\n");
			#endif
			return (-2);		
		}
	}else{
		#ifdef DEBUG
		perror("iniciarComunicacionMotores: No se logro inciar la comunicacion de los motores\n");
		#endif
		return (-1);
	}
}

/*******************************************************/

int asignarVelocidadLineal(int fd, double vl){
	if(vl > 0){
		if(vl > 1.016){
			vl = 1.016; 
		}else{
			if(vl < 0.008){
				vl = 0;
			}	
		}
	}else{
		if(vl < 0){
			if(vl < -1.016){
				vl = -1.016; 
			}else{
				if(vl > -0.008){
					vl = 0;
				}	
			}		
		}
	}
	int respuesta;

	vl = (vl/0.008);
	respuesta = asignarVelocidad1(fd,vl);
	if(respuesta == 0){
		return (0);
	}else{
		#ifdef DEBUG
		perror("asignarVelocidadLineal: No se logro asignar la velocidad lineal\n");
		#endif
		return (-1);
	}
}

/*******************************************************/

int obtenerVelocidadLineal(int fd, double *vl){
	int respuesta;	
	respuesta = obtenerVelocidad1(fd, vl);
	if(respuesta == 0 ){	
		*vl = (*vl)*0.008;
		return (0);	
	}else{
		#ifdef DEBUG
		perror("obtenerVelocidadLineal: No se logro obtener el valor de la velocidad lineal\n");
		#endif	
		return (-1);
	}
}

/*******************************************************/

int asignarVelocidadAngular(int fd, double w){
	if(w > 0){
		if(w > 7.69){
			w = 7.69; 
		}else{
			if(w < 0.061){
				w = 0;
			}	
		}
	}else{
		if(w < 0){
			if(w < -7.69){
				w = -7.69; 
			}else{
				if(w > -0.061){
					w = 0;
				}	
			}		
		}
	}
	int respuesta;
	w = ((w*LONGITUD_EJE*0.5)/0.008);
	respuesta = asignarVelocidad2(fd,w);
	if(respuesta == 0){
		return (0);
	}else{
		#ifdef DEBUG
			perror("asignarVelocidadAngular: No se logro asignar la velocidad angular\n");
		#endif
		return (-1);
	}
}

/*******************************************************/

int obtenerVelocidadAngular(int fd, double *w){ 
	int respuesta;	
	respuesta = obtenerVelocidad2(fd, w);
	if(respuesta == 0 ){
		*w = (((*w)*0.008)*2)/LONGITUD_EJE;
		return (0);	
	}else{
		#ifdef DEBUG
		perror("obtenerVelocidadAngular: No se logro obtener el valor de la velocidad angular\n");
		#endif	
		return (-1);
	}	
}

/*******************************************************/

int moverLineaRecta(int fd, double d, double vl){
	int pulsos, codificador1, codificador2, respuesta, aux=0;
	pulsos = numeroPulsos(d);
	if(d < 0) vl=(vl*-1);
	respuesta = enviarComando(fd, REINICIO_CODIFICADORES);
	respuesta = respuesta + asignarVelocidadLineal(fd, vl);
	do{
		respuesta = respuesta + obtenerCodificadorMotor1(fd, &codificador1);
		respuesta = respuesta + obtenerCodificadorMotor2(fd, &codificador2);
		if(d < 0 && codificador1 != 0) codificador1 = (0xffffffff) - codificador1;
		if(d < 0 && codificador2 != 0) codificador2 = (0xffffffff) - codificador2;
		if(((pulsos-codificador1) <= 180 || (pulsos-codificador2) <= 180) && aux == 0){
			if(vl > 0 && vl > VL_FINAL) vl = VL_FINAL;		
			if(vl < 0 && vl < (VL_FINAL*-1)) vl = (VL_FINAL*-1);
			respuesta = respuesta + asignarVelocidadLineal(fd, vl);
			aux = 1;
		} 
	}while(codificador1 < pulsos && codificador2 < pulsos); 
	respuesta = respuesta + asignarVelocidadLineal(fd,DETENER);
	if(respuesta == 0){ 
		return (0); 
	}else{
		#ifdef DEBUG
		perror("moverLineaRecta: No se logro realizar el movimiento\n");
		#endif	
		 return (-1);
	}
}

/*******************************************************/

int girar(int fd, double teta, double w){
	double dm;
	int pulsos, aux = 0, codificador2, codificador1, respuesta ,factor;
	teta = calcularTeta(teta);
	dm = (teta*LONGITUD_EJE)/2;
	pulsos = numeroPulsos(dm);
	if(teta > 0) w = w*(-1);
	respuesta = enviarComando(fd, REINICIO_CODIFICADORES);
	respuesta = respuesta + asignarVelocidadAngular(fd, w);
	do{
		respuesta = respuesta + obtenerCodificadorMotor1(fd, &codificador1);
		respuesta = respuesta + obtenerCodificadorMotor2(fd, &codificador2);
		if(teta < 0){		
			if(codificador2 != 0) codificador2 = (0xffffffff) - codificador2;
		}else{
			if(codificador1 != 0) codificador1 = (0xffffffff) - codificador1;
		}
		if(((pulsos - codificador1) <= 90 || (pulsos - codificador2) <= 90 ) && (fabs(w) > W_FINAL) && (aux == 0)){
			w = W_FINAL*(w/fabs(w));
			respuesta = respuesta + asignarVelocidadAngular(fd, w);
			aux = 1;
		}
	}while(codificador1 < pulsos && codificador2 < pulsos);
	respuesta = respuesta + asignarVelocidadAngular(fd, DETENER);
	if(respuesta == 0){ 
		return (0); 
	}else{
		#ifdef DEBUG
		perror("giroRelativo: No se logro realizar el giro\n");
		#endif	
		return (-1);
	}	
}

/*******************************************************/

int gotoXY(int fd, struct datosCinematica *estadoActual, struct datosCinematica estadoNuevo){
	int respuesta;	
	double distancia, cOpuesto, cAdyacente ,tetaAuxiliar, tetaGiro;
	cOpuesto = estadoNuevo.y - estadoActual->y;
	cAdyacente = estadoNuevo.x - estadoActual->x;
	distancia = sqrt(pow(cAdyacente, 2) + pow(cOpuesto,2));
	tetaAuxiliar = asin(cOpuesto/distancia);
	tetaAuxiliar = calculoCuadrante(tetaAuxiliar, cAdyacente,  cOpuesto);
	tetaGiro = tetaAuxiliar - estadoActual->theta;
	respuesta = girar(fd, tetaGiro, 1.5);
	estadoActual->theta = tetaAuxiliar;
	usleep(500000);
	respuesta = respuesta + moverLineaRecta(fd, distancia, 0.3);
	tetaGiro = estadoNuevo.theta - estadoActual->theta;
	usleep(500000);
	respuesta = respuesta + girar(fd, tetaGiro, 1.5);
	if(respuesta == 0){ 
		estadoActual->x = estadoNuevo.x;
		estadoActual->y = estadoNuevo.y;
		estadoActual->theta = estadoNuevo.theta;
		return (0); 
	}else{
		#ifdef DEBUG
		perror("gotoXY: No se logro ir a la posicion (x,y,teta)\n");
		#endif	
		return (-1);
	}	
}

/*******************************************************/

double calculoCuadrante(double teta, double x, double y){
	if(y >= 0){
		if(x >= 0){
			return teta;		
		}else{
			teta = (M_PI - teta);
			return teta;
		}
	}else{
		if(x >= 0){
			teta = (2*M_PI + teta);
			return teta;		
		}else{
			teta = (M_PI - teta);
			return teta;
		}
	}
}

/*******************************************************/

int terminarComunicacionMotores(int fd){
	int respuesta;
	respuesta = asignarVelocidadAngular(fd, DETENER);
	respuesta = respuesta + asignarVelocidadLineal(fd, DETENER);
	respuesta = respuesta + cerrarPuerto(fd);
	if(respuesta == 0){ 
		return (0); 
	}else{
		#ifdef DEBUG
		perror("terminarComunicacionMotores: La comunicacion no se termino con exito\n");
		#endif	
		return (-1);
	}	
}

/*******************************************************/

int numeroPulsos(double d){
	double cm;
	cm = (M_PI*DIAMETRO)/(N_REDUCCION*PULSOS_REVOLUCION);
	return (int)(fabs(d)/cm);
}

/*******************************************************/

double calcularTeta(double teta){
	if(teta > (2*M_PI) || teta < (-2*M_PI)){
		teta = teta - (int)(teta/(2*M_PI))*(2*M_PI);
	}
	if(teta > M_PI || teta < -M_PI){
		teta = teta-((2*M_PI)*(teta/fabs(teta)));
	}
	return teta;
}

